#include "GearBox.h"
#include "../Robotmap.h"
#include "../Commands/GearBoxIdle.h"

GearBox::GearBox() : Subsystem("GearBox") {
	
	gear = new DoubleSolenoid(RobotMap::SOLENOID_MODULE_SLOT, 
							  RobotMap::PNEU_HIGH_GEAR_POS, 
							  RobotMap::PNEU_LOW_GEAR_POS);
	
}

void GearBox::InitDefaultCommand()
{
	SetDefaultCommand(new GearBoxIdle());
}


void GearBox::shiftUp() {
	gear->Set(DoubleSolenoid::kForward);
	selectedGear = HIGH_GEAR;
}

void GearBox::shiftDown() {
	gear->Set(DoubleSolenoid::kReverse);
	selectedGear = LOW_GEAR;
}

void GearBox::valveOff() {
	gear->Set(DoubleSolenoid::kOff);
}

void GearBox::doNothing() {
	
}

int GearBox::getSelectedGear() {
	return selectedGear;
}

char* GearBox::getSelectedGearName() {
	return translateGear();
}

char* GearBox::translateGear() {
	switch(selectedGear) {
	case HIGH_GEAR:
		return "High gear";
	case LOW_GEAR:
		return "Low gear";
	default:
		return "UnKnown";
	}
}
